#include "hexagon_types.h"
#include "hexagon_protos.h"
#include "q6cache.h"
#include "dsp_intrinsics.hpp"

#if defined(V5_4)
#undef V5_4
#endif
#include "hvx_vector_register_defines.h"

#include "qfe.h"
#include "cv_mat.hpp"

#define VLEN 128
// int32_t bitrev(int32_t x, int32_t bits)
// {
//     int32_t i;
//     int32_t y = 0;

//     for (i = 0; i < bits; i++)
//     {
//         y = (y << 1) | (x & 1);
//         x >>= 1;
//     }

//     return y;
// }

void cvmat_hfq15_dot(CvMat_hfq15 &x1, CvMat_hfq15 &x2, wfq15_t *out)
{
    long long l2fetch1 = CreateL2pfParam(x1.colStride(), x1.colStride(), 1, 0);
    long long l2fetch2 = CreateL2pfParam(x2.colStride(), x2.colStride(), 1, 0);
    hfq15_t *pSrc1 = x1.data;
    hfq15_t *pSrc2 = x2.data;
    L2fetch( (unsigned int)(pSrc1), l2fetch1);
    L2fetch( (unsigned int)(pSrc2), l2fetch2);
    HVX_VP V1__0, V3__2;
    HVX_VectorPair vp_zero = Q6_W_vzero();
    HVX_Vector v_out = Q6_V_vzero();
    HVX_Vector v_zero = Q6_V_vzero();
    int32_t __attribute__((aligned(VLEN))) tmp_buf[32];
    HVX_Vector *p_v1 = (HVX_Vector*)pSrc1;
    HVX_Vector *p_v2 = (HVX_Vector*)pSrc2;
    for (int i = 0; i < x1.rows; i++) {
        if (i < x1.rows-1) {
            L2fetch( (unsigned int)(pSrc1+x1.cols), l2fetch1);
            L2fetch( (unsigned int)(pSrc2+x2.cols), l2fetch2);
        }
        for (int j = 0; j < x1.cols; j += 64) {
            V0 = *p_v1++;
            V1_0 = Q6_Ww_vunpack_Vh(V0);
            V2 = *p_v2++;
            V3_2 = Q6_Ww_vunpackoor_WwVh(vp_zero, V2);
            V2 = Q6_Vw_vmpyo_VwVh_s1_sat(V0, V2);
            v_out = Q6_Vw_vadd_VwVw_sat(v_out, V2);
            V3 = Q6_Vw_vmpyo_VwVh_s1_sat(V1, V3);
            v_out = Q6_Vw_vadd_VwVw_sat(v_out, V3);
        }
        pSrc1 += x1.cols;
        pSrc2 += x2.cols;
    }
 
    for (int32_t i = 64; i >= 4; i >>= 1)
    {
        v_out = Q6_Vw_vadd_VwVw(v_out, Q6_V_vlalign_VVR(v_out, v_zero, i));
    }

    *(HVX_Vector *) tmp_buf = v_out;
    *out = tmp_buf[31];   
}

void cvmat_cwfq16_mul(CvMat_cwfq16 &x1_c, CvMat_cwfq16 &x2_c, CvMat_cwfq8 &c)
{
    long long l2fetch1 = CreateL2pfParam(x1_c.colStride(), x1_c.colStride(), 1, 0);
    long long l2fetch2 = CreateL2pfParam(x2_c.colStride(), x2_c.colStride(), 1, 0);
    cwfq16_t *pSrc1 = x1_c.data;
    cwfq16_t *pSrc2 = x2_c.data;
    cwfq16_t *pDst = c.data;
    L2fetch( (unsigned int)(pSrc1), l2fetch1);
    L2fetch( (unsigned int)(pSrc2), l2fetch2);
    HVX_VP V1__0, V3__2, V5__4, V7__6, V9__8, V11__10, V13__12;
    HVX_VectorPair *p_pv1 = (HVX_VectorPair*)pSrc1;
    HVX_VectorPair *p_pv2 = (HVX_VectorPair*)pSrc2;
    HVX_VectorPair *p_pv3 = (HVX_VectorPair*)pDst;
    for (int i = 0; i < c.rows; i++) {
        if (i < c.rows-1) {
            L2fetch( (unsigned int)(pSrc1+x1_c.cols), l2fetch1);
            L2fetch( (unsigned int)(pSrc2+x2_c.cols), l2fetch2);
        }
        // (R1+I1i)*(R2+I2i)=(R1*R2-I1*I2)+(R1*I2+R2*I1)i
        for (int j = 0; j < c.cols; j += 32) {
            V1_0 = *p_pv1++;
            V3_2 = Q6_W_vdeal_VVR(V1, V0, -4); // V2: real, V3: imag 
            V1_0 = *p_pv2++;
            V5_4 = Q6_W_vdeal_VVR(V1, V0, -4); // V4: real, V5: imag 
            // R1*R2 // int64
            V7_6 = Q6_W_vmpye_VwVuh(V4, V2); 
            V7_6 = Q6_W_vmpyoacc_WVwVh(V7_6, V4, V2);
            // to int32
            V6 = Q6_Vuw_vlsr_VuwR(V6, 24);
            V6 = Q6_Vw_vaslacc_VwVwR(V6, V7, 8);
            // I1*I2 // int64
            V9_8 = Q6_W_vmpye_VwVuh(V5, V3); 
            V9_8 = Q6_W_vmpyoacc_WVwVh(V9_8, V5, V3);
            // // int32
            V8 = Q6_Vuw_vlsr_VuwR(V8, 24);
            V8 = Q6_Vw_vaslacc_VwVwR(V8, V9, 8);
            // R1*I2 // int64
            V11_10 = Q6_W_vmpye_VwVuh(V2, V5); 
            V11_10 = Q6_W_vmpyoacc_WVwVh(V11_10, V2, V5);
            // // int32
            V10 = Q6_Vuw_vlsr_VuwR(V10, 24);
            V10 = Q6_Vw_vaslacc_VwVwR(V10, V11, 8);
            // R2*I1 // int64
            V13_12 = Q6_W_vmpye_VwVuh(V4, V3); 
            V13_12 = Q6_W_vmpyoacc_WVwVh(V13_12, V4, V3);
            // // int32
            V12 = Q6_Vuw_vlsr_VuwR(V12, 24);
            V12 = Q6_Vw_vaslacc_VwVwR(V12, V13, 8);
            // R=R1*R2-I1*I2 // int32
            V0 = Q6_Vw_vsub_VwVw_sat(V6, V8);
            // I=R1*I2+R2*I1
            V1 = Q6_Vw_vadd_VwVw_sat(V12, V10);
            // (I,R)
            V1_0 = Q6_W_vshuff_VVR(V1, V0, -4); 
            *p_pv3++ = V1_0;
        }
        pSrc1 += x1_c.cols;
        pSrc2 += x2_c.cols;
    }
}

void cvmat_cwfq16_mul_conj(CvMat_cwfq16 &x1_c, CvMat_cwfq16 &x2_c, CvMat_cwfq7 &c)
{
    long long l2fetch1 = CreateL2pfParam(x1_c.colStride(), x1_c.colStride(), 1, 0);
    long long l2fetch2 = CreateL2pfParam(x2_c.colStride(), x2_c.colStride(), 1, 0);
    cwfq16_t *pSrc1 = x1_c.data;
    cwfq16_t *pSrc2 = x2_c.data;
    cwfq16_t *pDst = c.data;
    L2fetch( (unsigned int)(pSrc1), l2fetch1);
    L2fetch( (unsigned int)(pSrc2), l2fetch2);
    HVX_VP V1__0, V3__2, V5__4, V7__6, V9__8, V11__10, V13__12;
    HVX_VectorPair *p_pv1 = (HVX_VectorPair*)pSrc1;
    HVX_VectorPair *p_pv2 = (HVX_VectorPair*)pSrc2;
    HVX_VectorPair *p_pv3 = (HVX_VectorPair*)pDst;
    for (int i = 0; i < c.rows; i++) {
        if (i < c.rows-1) {
            L2fetch( (unsigned int)(pSrc1+x1_c.cols), l2fetch1);
            L2fetch( (unsigned int)(pSrc2+x2_c.cols), l2fetch2);
        }
        // (R1+I1i)*conj(R2+I2i)=(R1*R2+I1*I2)+(R2*I1-R1*I2)i
        for (int j = 0; j < c.cols; j += 32) {
            V1_0 = *p_pv1++;
            V3_2 = Q6_W_vdeal_VVR(V1, V0, -4); // V2: real, V3: imag 
            V1_0 = *p_pv2++;
            V5_4 = Q6_W_vdeal_VVR(V1, V0, -4); // V4: real, V5: imag 
            // R1*R2 // int64
            V7_6 = Q6_W_vmpye_VwVuh(V4, V2); 
            V7_6 = Q6_W_vmpyoacc_WVwVh(V7_6, V4, V2);
            // to int32
            V6 = Q6_Vuw_vlsr_VuwR(V6, 25);
            V6 = Q6_Vw_vaslacc_VwVwR(V6, V7, 7);
            // I1*I2 // int64
            V9_8 = Q6_W_vmpye_VwVuh(V5, V3); 
            V9_8 = Q6_W_vmpyoacc_WVwVh(V9_8, V5, V3);
            // // int32
            V8 = Q6_Vuw_vlsr_VuwR(V8, 25);
            V8 = Q6_Vw_vaslacc_VwVwR(V8, V9, 7);
            // R1*I2 // int64
            V11_10 = Q6_W_vmpye_VwVuh(V2, V5); 
            V11_10 = Q6_W_vmpyoacc_WVwVh(V11_10, V2, V5);
            // // int32
            V10 = Q6_Vuw_vlsr_VuwR(V10, 25);
            V10 = Q6_Vw_vaslacc_VwVwR(V10, V11, 7);
            // R2*I1 // int64
            V13_12 = Q6_W_vmpye_VwVuh(V4, V3); 
            V13_12 = Q6_W_vmpyoacc_WVwVh(V13_12, V4, V3);
            // // int32
            V12 = Q6_Vuw_vlsr_VuwR(V12, 25);
            V12 = Q6_Vw_vaslacc_VwVwR(V12, V13, 7);
            // R=R1*R2+I1*I2 // int32
            V0 = Q6_Vw_vadd_VwVw_sat(V6, V8);
            // I=R2*I1-R1*I2
            V1 = Q6_Vw_vsub_VwVw_sat(V12, V10);
            // (I,R)
            V1_0 = Q6_W_vshuff_VVR(V1, V0, -4); 
            *p_pv3++ = V1_0;
        }
        pSrc1 += x1_c.cols;
        pSrc2 += x2_c.cols;
    }
}
void cvmat_cwfq16_mul_conj1(CvMat_cwfq16 &x1_c, CvMat_cwfq16 &x2_c, CvMat_cwfq8 &c)
{
    long long l2fetch1 = CreateL2pfParam(x1_c.colStride(), x1_c.colStride(), 1, 0);
    long long l2fetch2 = CreateL2pfParam(x2_c.colStride(), x2_c.colStride(), 1, 0);
    cwfq16_t *pSrc1 = x1_c.data;
    cwfq16_t *pSrc2 = x2_c.data;
    cwfq16_t *pDst = c.data;
    L2fetch( (unsigned int)(pSrc1), l2fetch1);
    L2fetch( (unsigned int)(pSrc2), l2fetch2);
    HVX_VP V1__0, V3__2, V5__4, V7__6, V9__8, V11__10, V13__12;
    HVX_VectorPair *p_pv1 = (HVX_VectorPair*)pSrc1;
    HVX_VectorPair *p_pv2 = (HVX_VectorPair*)pSrc2;
    HVX_VectorPair *p_pv3 = (HVX_VectorPair*)pDst;
    HVX_VectorPred Qp;
    for (int i = 0; i < c.rows; i++) {
        if (i < c.rows-1) {
            L2fetch( (unsigned int)(pSrc1+x1_c.cols), l2fetch1);
            L2fetch( (unsigned int)(pSrc2+x2_c.cols), l2fetch2);
        }
        // (R1+I1i)*conj(R2+I2i)=(R1*R2+I1*I2)+(R2*I1-R1*I2)i
        for (int j = 0; j < c.cols; j += 32) {
            V1_0 = *p_pv1++;
            V3_2 = Q6_W_vdeal_VVR(V1, V0, -4); // V2: real, V3: imag 
            V1_0 = *p_pv2++;
            V5_4 = Q6_W_vdeal_VVR(V1, V0, -4); // V4: real, V5: imag 
            // R1*R2 // int64
            V7_6 = Q6_W_vmpye_VwVuh(V4, V2); 
            V7_6 = Q6_W_vmpyoacc_WVwVh(V7_6, V4, V2);
            // I1*I2 // int64
            V9_8 = Q6_W_vmpye_VwVuh(V5, V3); 
            V9_8 = Q6_W_vmpyoacc_WVwVh(V9_8, V5, V3);
            // R1*I2 // int64
            V11_10 = Q6_W_vmpye_VwVuh(V2, V5); 
            V11_10 = Q6_W_vmpyoacc_WVwVh(V11_10, V2, V5);
            // R2*I1 // int64
            V13_12 = Q6_W_vmpye_VwVuh(V4, V3); 
            V13_12 = Q6_W_vmpyoacc_WVwVh(V13_12, V4, V3);
            // R=R1*R2+I1*I2 // int32
            V6 = Q6_Vw_vadd_VwVwQ_carry(V6, V8, &Qp);
            V7 = Q6_Vw_vadd_VwVwQ_carry_sat(V7, V9, Qp);
            // I=R2*I1-R1*I2
            V12 = Q6_Vw_vsub_VwVwQ_carry(V12, V10, &Qp);
            V13 = Q6_Vw_vsub_VwVw_sat(V13, V11);
            // // int32
            V6 = Q6_Vuw_vlsr_VuwR(V6, 24);
            V6 = Q6_Vw_vaslacc_VwVwR(V6, V7, 8);
            V12 = Q6_Vuw_vlsr_VuwR(V12, 24);
            V12 = Q6_Vw_vaslacc_VwVwR(V12, V13, 8);
            // (I,R)
            V1_0 = Q6_W_vshuff_VVR(V12, V6, -4); 
            *p_pv3++ = V1_0;
        }
        pSrc1 += x1_c.cols;
        pSrc2 += x2_c.cols;
    }
}